#include "pid_app.h"
#include "./lw_pid/lw_pid.h"
#include "./encorder/encorder.h"
#include "./imu/imu.h"
#include "./motor/motor.h"
#include "./OLED/OLED.h"
#include "./user_usart.h"

/********************全局变量定义********************/
Pid_object_t r_speed_pid; /*右轮速度环*/
Pid_object_t l_speed_pid; /*左轮速度环*/
Pid_object_t angle_pid;   /*角度环*/

/********************全局函数定义********************/

/**
 * @note 需要依次设置好参数，目标值，输出限幅使用，输入死区使用，输出偏移使用
     积分限幅使用（仅位置式），增量输出全量（仅增量式），不需要的功能一定要显式禁用，避免意外
 */
void autocar_pid_init(void)
{
	pid_set_argument(&r_speed_pid, 7.00f, 0.07f, 0.00f); 	  /*设置参数*/
	pid_set_target(&r_speed_pid, 0.0);                   	  /*设置目标值*/
	pid_output_limit_disable(&r_speed_pid); 				  /*禁用输出限幅*/
	pid_output_offset_enable(&r_speed_pid);                   /*使能输出偏移*/
	pid_set_output_offset(&r_speed_pid, 300.0f);              /*设置输出偏移*/
	pid_intergral_limit_enable(&r_speed_pid);                 /*启用积分限幅*/
	pid_set_intergral_limit(&r_speed_pid, 1800.0f, -1800.0f); /*设置积分限幅*/
	pid_input_dead_zone_disable(&r_speed_pid);                /*禁用输入死区*/
	pid_set_mode_fullscale(&r_speed_pid);                     /*设置为位置式pid*/
	pid_init(&r_speed_pid);
	
	pid_set_argument(&l_speed_pid, 7.00f, 0.07f, 0.00f); 	  /*设置参数*/
	pid_set_target(&l_speed_pid, 0.0);                  	  /*设置目标值*/
	pid_output_limit_disable(&l_speed_pid); 				  /*禁用输出限幅*/
	pid_output_offset_enable(&l_speed_pid);                   /*使能输出偏移*/
	pid_set_output_offset(&l_speed_pid, 300.0f);              /*设置输出偏移*/
	pid_intergral_limit_enable(&l_speed_pid);                 /*启用积分限幅*/
	pid_set_intergral_limit(&l_speed_pid, 1800.0f, -1800.0f); /*设置积分限幅*/
	pid_input_dead_zone_disable(&l_speed_pid);                /*禁用输入死区*/
	pid_set_mode_fullscale(&l_speed_pid);                     /*设置为位置式pid*/
	pid_init(&l_speed_pid);
	
	pid_set_argument(&angle_pid, 0.42f, 0.00f, 0.0f);              /*设置参数*/
	pid_set_target(&angle_pid, 0.0f);                              /*设置目标值*/
	pid_output_limit_enable(&angle_pid);                           /*使能输出限幅*/
	pid_set_output_limit(&angle_pid, 70.0f + 0.0f, -70.0f - 0.0f); /*设置输出限幅*/
	pid_output_offset_enable(&angle_pid);                          /*使能输出偏移*/
	pid_set_output_offset(&angle_pid, 25.0f);                      /*设置输出偏移*/
	pid_input_dead_zone_enable(&angle_pid);                        /*使能输入死区*/
	pid_set_input_dead_zone(&angle_pid, 0.5f);					   /*设置输入死区*/
	pid_set_mode_fullscale(&angle_pid); 						   /*设置为位置式pid*/
	pid_init(&angle_pid);
	
	pid_disable(&angle_pid); /*默认先失能角度环*/
}

/**
 * @brief	pid控制任务函数
 */
void pid_ctrl_handler(void)
{
  int32_t r_motor_pulse = 0, l_motor_pulse = 0;
  int8_t imu_state = 0;
  float yaw_angle = 0.0f;
  float yaw_angle_error = 0.0f;
  imu_euler angle = {0};
  float r_speed_pid_res = 0.0f, l_speed_pid_res = 0.0f;
  static float angle_pid_res = 0.0f;
  static uint8_t angle_pid_tick = 1;

  /*获取速度值*/
  r_motor_pulse = r_motor_get_pulse();
  l_motor_pulse = l_motor_get_pulse();

  /*获取偏航角*/
  imu_state = imu_get_data(&angle, false);

  OLED_ShowSignedNum(0, 40, r_motor_pulse, 4, OLED_6X8);
  OLED_ShowSignedNum(0, 48, l_motor_pulse, 4, OLED_6X8);
  
  if (imu_state == IMU_OK && angle_pid_tick == 0)
  {
    angle_pid_tick = 1;
    OLED_ShowFloatNum(48, 40, angle.yaw, 3, 4, OLED_6X8);

    /*获取实际误差值*/
    yaw_angle_error = imu_math_cal_euler_error(angle_pid.target, angle.yaw);
    yaw_angle = angle_pid.target + (-yaw_angle_error);

    /*计算角度环*/
    angle_pid_res = pid_calcu(&angle_pid, yaw_angle);
  }
  else if (angle_pid_tick != 0)
  {
    angle_pid_tick--;
  }

  /*计算速度环*/
  r_speed_pid_res = pid_calcu(&r_speed_pid, (float)r_motor_pulse - angle_pid_res); // angle_pid_res*2
  l_speed_pid_res = pid_calcu(&l_speed_pid, (float)l_motor_pulse + angle_pid_res); // 0.0f

  r_motor_pwm_set((int32_t)r_speed_pid_res);
  l_motor_pwm_set((int32_t)l_speed_pid_res);

  /*关闭测速中断，开始进入临界区*/
  motor_speed_measure_int_disable();
  r_motor_set_pulse(0); /*清空计数值*/
  l_motor_set_pulse(0);
  motor_speed_measure_int_enable();
  /*开启测速中断，退出临界区*/
}
